﻿// test_motion9.cpp : 定义控制台应用程序的入口点。
//
#ifdef WIN32
#include <winsock2.h> 
#include <Windows.h>
#include <Mmsystem.h>
#include "stdafx.h"
#endif
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "motion9_dll.h"
#include "platform.h"
#ifdef WIN32
#pragma  comment(lib,"ws2_32.lib")
#pragma comment(lib, "Winmm.lib") 
#endif
FILE *inter_out;
FILE *test_fd;
//#define SIMULATE 
//初始化网络
SOCKET sclient = NULL;
double current_position[MAX_JOINT];
int InitTcpIp()
{
#ifdef WIN32
	WORD sockVersion = MAKEWORD(2, 2);
	WSADATA data;
	if (WSAStartup(sockVersion, &data) != 0){
		return 0;
	}
	sclient = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
	if (sclient == INVALID_SOCKET){
		printf("invalid socket !");
		return 0;
	}

	sockaddr_in serAddr;
	serAddr.sin_family = AF_INET;
	serAddr.sin_port = htons(6767);
	serAddr.sin_addr.S_un.S_addr = inet_addr("127.0.0.1");
	if (connect(sclient, (sockaddr *)&serAddr, sizeof(serAddr)) == SOCKET_ERROR){
		printf("connect error !");
		closesocket(sclient);
		return 0;
	}
	char * sendData = "你好，TCP服务端，我是客户端!\n";
	send(sclient, sendData, strlen(sendData), 0);
#endif	
	return 0;
}

//消息回调
int MsgCallback(int reason,char *msg)
{
	if (reason < 2000) printf("reason=%d\n", reason);
	else if (reason < 3000)printf("reason=%d,msg:%s", reason,msg);
	return 0;
}
//位置输出回调
int OutCallback(double posit[MAX_JOINT], float vel[MAX_JOINT], float acc[MAX_JOINT])
{
	memcpy(current_position, posit, sizeof(double)*MAX_JOINT);
#if 1
	fprintf(inter_out, "%.6f\n", posit[0]);
	//STRUCT_POSE pose;
	//Motion_KmClampForward(posit, &pose, NULL);
	//fprintf(inter_out,"%.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\n",
	//	//posit[0],posit[1],posit[2],posit[3],
	//	//posit[4],posit[5],posit[6],posit[7],posit[8]
	//	vel[0], vel[1], vel[2], vel[3],
	//	pose.x,pose.y,pose.z,pose.rz,0.0
	//	);
	fflush(inter_out);
#endif

#if 1
	fprintf(stdout, "%.6f %.6f %.6f %.6f %.3f %.3f %.3f %.3f %.3f %.3f\n",
		posit[0], posit[1], posit[2], posit[3],
		//posit[4],posit[5],posit[6],posit[7],posit[8]
		vel[0], vel[1], vel[2], vel[3],0.0
		//pose.x,pose.y,pose.z,pose.rz,0.0
		);
	//fprintf(inter_out, "%.6f %.6f %.6f %.6f %.3f %.3f %.3f %.3f %.3f %.3f\n",
	//	posit[0], posit[1], posit[2], posit[3],
	//	//posit[4],posit[5],posit[6],posit[7],posit[8]
	//	vel[0], vel[1], vel[2], vel[3],0.0
	//	//pose.x,pose.y,pose.z,pose.rz,0.0
	//	);
#endif

#if 0
	DWORD counter = timeGetTime();
	printf("%d\n", counter);
#endif
	return 0;
}

//Motion计算线程
DWORD WINAPI Serv(LPVOID pArg)
{
	while (1){
		C_SLEEP_MS(1);
		Motion_Service();
	}
	return 0;
}
DWORD WINAPI Pout(LPVOID pArg)
{
	while (1){
		C_SLEEP_MS(1.9);
		Motion_Out();
	}
	return 0;
}
DWORD WINAPI ToSim(LPVOID pArg)
{
#ifdef SIMULATE
	char SendData[24] = { 0 };
	float float_posit;
	while (1){
		for (int i = 0; i < 4; i++){
			float_posit = (float)current_position[i];
			if (i == 2) float_posit /= 200;
			SendData[i * 4 + 0] = *(((char *)&float_posit) + 3);
			SendData[i * 4 + 1] = *(((char *)&float_posit) + 2);
			SendData[i * 4 + 2] = *(((char *)&float_posit) + 1);
			SendData[i * 4 + 3] = *(((char *)&float_posit) + 0);
		}
		if (sclient != 0)send(sclient, SendData, 24, 0);
		C_SLEEP_MS(20);
	}
#endif
	return 0;
}

//初始化
int initial_motion()
{
	STRUCT_MOTION_PARM para = { 0 };
	//结构和速度配置
	float max_vel[MAX_JOINT] = { 3.8765, 6.1569, 500, 6.1569, 10, 10, 10, 10, 10 };
	float max_acc[MAX_JOINT] = { 3.8765 * 5, 6.1569 * 5, 500 * 5, 6.1569*5, 100, 100, 100, 100, 100 };
	double abs_limit_h[MAX_JOINT] = { 2.8, 2.5, 150, 6.28, 0, 0, 0, 0, 0 };
	double abs_limit_l[MAX_JOINT] = { -2.8, -2.5, -150, -6.28, 0, 0, 0, 0, 0 };
	double soft_limit_h[MAX_JOINT] = { 2.5, 2.0, 120, 6.0, 0, 0, 0, 0, 0 };
	double soft_limit_l[MAX_JOINT] = { -2.5, -2.0, -120, -6.0, 0, 0, 0, 0, 0 };
	para.machine_struct = SCARA;
	para.dh_machine[0].a = 250;
	para.dh_machine[0].d = 0;
	para.dh_machine[1].a = 150;
	para.callback_msg = MsgCallback;
	para.callback_out = OutCallback;
	memcpy(para.vel_joint.vel_max, max_vel, sizeof(float)*MAX_JOINT);
	memcpy(para.vel_joint.acc_max, max_acc, sizeof(float)*MAX_JOINT);
	para.vel_line.vel_max = 2000;
	para.vel_line.acc_max = 50000;
	para.vel_rot.vel_max = 10;
	para.vel_rot.acc_max = 10;
	//位置限制配置
	memcpy(para.limit_abs.limit_h, abs_limit_h,sizeof(double)*MAX_JOINT);
	memcpy(para.limit_abs.limit_l, abs_limit_l, sizeof(double)*MAX_JOINT);
	memcpy(para.limit_soft.limit_h, abs_limit_h, sizeof(double)*MAX_JOINT);
	memcpy(para.limit_soft.limit_l, abs_limit_l, sizeof(double)*MAX_JOINT);
	para.inter_period = (float)0.02;
	para.joint_period = (float)0.002;
	para.fd1 = test_fd;
	Motion_Init(para);
	return 0;
}

//
int test_free()
{
	STRUCT_USER_VEL vel;
	vel.jv.vel = 1.0;
	vel.jv.acca = 0.1;
	vel.jv.accd = 0.1;
	vel.lv.vel = 500;
	vel.lv.acca = 10000;
	vel.lv.accd = 10000;
	vel.rv.vel = 0.5;
	vel.rv.acca = 5;
	vel.rv.accd = 5;
	STRUCT_POSE pose1 = { 0 };
	pose1.x = 200;
	pose1.y = -100;
	STRUCT_POSE pose2 = { 0 };
	pose2.x = 200;
	pose2.y = 100;
	STRUCT_POSE pose3 = { 0 };
	pose3.x = 300;
	pose3.y = 100;
	STRUCT_POSE pose4 = { 0 };
	pose4.x = 300;
	pose4.y = -100;
	double posit[9] = { 2.2, 0, 0, 0, 0, 0, 0, 0, 0 };
	double posit1[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
#if 1
	for (int i = 0; i < 1; i++){
		Motion_Add_Joint_Free_Jog_Abstract_Linked(1, posit, vel, 0, 1);
		Motion_WaitMoveDone(10, 100000);
		Motion_Add_Joint_Free_Jog_Abstract_Linked(1, posit1, vel, 0, 1);
		Motion_WaitMoveDone(10, 100000);
	}
#endif
#if 0
	for (int i = 0; i < 1; i++){
		Motion_Add_Axis_Free_Jog_Abstract(1, pose1, 0, vel, 1, 1);
		//Motion_WaitMoveDone(10, 100000);
		//Motion_Add_Axis_Free_Jog_Abstract(1, pose2, 1, vel, 1, 1);
		//Motion_WaitMoveDone(10, 100000);
		//Motion_Add_Axis_Free_Jog_Abstract(1, pose3, 0, vel, 1, 1);
		//Motion_WaitMoveDone(10, 100000);
		//Motion_Add_Axis_Free_Jog_Abstract(1, pose4, 0, vel, 1, 1);
		//Motion_WaitMoveDone(10, 100000);
	}
#endif
	return 0;
}

int test_plane_p2p_posit()
{
	STRUCT_USER_VEL vel = {
		{ 0.4, 1.0, 1.0},
		{500,10000,10000},
		{0.5,5,5}
	};
	STRUCT_USER_OVL ovl = {1,0,0};
	int id = 0;
	double posit1[MAX_JOINT] = { 0.5, 0.5, 0.0,  0.0};
	double posit2[MAX_JOINT] = { 2.0, 1.0, 50.0,  0.0 };
	double posit3[MAX_JOINT] = { 1.0, 1.0, 0.0, 0.0};
	double posit4[MAX_JOINT] = { -1.0,-1.0, 0.0, 0.0 };
	double posit5[MAX_JOINT] = { -1.0, -1.0, 50.0, 0.0 };
	double posit6[MAX_JOINT] = { -1.0, -1.0, 0.0, 0.0 };
	for (int i = 0; i < 1; i++){
		Motion_AddPlaneP2P_Posit(posit1, vel, ovl, ++id, 1);
		//Motion_AddPlaneP2P_Posit(posit2, vel, ovl, ++id, 1);
		//Motion_AddPlaneP2P_Posit(posit3, vel, ovl, ++id, 1);
		//Motion_AddPlaneP2P_Posit(posit4, vel, ovl, ++id, 1);
		//Motion_AddPlaneP2P_Posit(posit5, vel, ovl, ++id, 1);
		//Motion_AddPlaneP2P_Posit(posit6, vel, ovl, ++id, 1);
		//int ttt = rand() % 4000;
		//printf("t=%d\n", ttt);
		//Sleep(ttt);
		//Motion_CommandPause();
		//Sleep(rand() % 4000);
		//Motion_CommandResume();
		Motion_WaitMoveDone(10, 100000);
	}
	return 0;
}

int test_plane_p2p_pose()
{
	STRUCT_USER_VEL vel = {
		{ 1.0, 1.0, 1.0 },
		{ 500, 10000, 10000 },
		{ 0.5, 5, 5 }
	};
	STRUCT_USER_OVL ovl = { 1, 0, 0 };
	int id = 0;
	STRUCT_POSE pose1 = { 200, -200, 0, 0, 0, 0 };
	STRUCT_POSE pose2 = { 200, 200, 0, 0, 0, 0 };
	STRUCT_POSE pose3 = { 300, 200, 0, 0, 0, 0 };
	STRUCT_POSE pose4 = { 300, -200, 0, 0, 0, 0 };
	STRUCT_POSE pose5 = { 300, -200, 0, 0, 0, 0 };
	STRUCT_POSE pose6 = { 200, -200, 0, 0, 0, 0 };
	for (int i = 0; i < 100; i++){
		Motion_AddPlaneP2P_Pose(pose1, 0, vel, ovl, ++id, 1);
		Motion_AddPlaneP2P_Pose(pose2, 0, vel, ovl, ++id, 1);
		Motion_AddPlaneP2P_Pose(pose3, 0, vel, ovl, ++id, 1);
		Motion_AddPlaneP2P_Pose(pose4, 0, vel, ovl, ++id, 1);
		Motion_AddPlaneP2P_Pose(pose5, 0, vel, ovl, ++id, 1);
		//int ttt = rand() % 4000;
		//printf("t=%d\n", ttt);
		//Sleep(ttt);
		//Motion_CommandPause();
		//Sleep(rand() % 4000);
		//Motion_CommandResume();
		Motion_WaitMoveDone(10, 100000);
	}
	return 0;
}

int test_fkine()
{
	double posit[MAX_JOINT] = { -0.5, 0.5, 0, 0, 0, 0, 0, 0, 0 };
	STRUCT_POSE pose_curr;
	STRUCT_POSE pose_last;
	for (int i = 0; i<10000000;i++){
		//sleep_ms(1);
		Motion_KmClampForward(posit, &pose_curr, NULL);
		Motion_KmClampForward(posit, &pose_last, NULL);
		if (fabs(pose_last.x - pose_curr.x) > 1e-3 ||
			fabs(pose_last.y - pose_curr.y) > 1e-3 ||
			fabs(pose_last.z - pose_curr.z) > 1e-3){
			printf("check line last pose and current pose faild\n");
		}
		printf("ture i=%d\n",i);
	}
	while (1){
		sleep_ms(1);
	}
	return 0;
}

int test_plane_line_pose()
{
	STRUCT_USER_VEL vel = {
		{ 1.0, 1.0, 1.0, 0},
		{ 500, 1000, 1000, 0},
		{ 0.5, 5, 5, 0}
	};
	STRUCT_USER_OVL ovl = { 1, 0, 0 };
	int id = 0;
	STRUCT_POSE pose1 = { 200, -200, 0, 0, 0, 0 };
	STRUCT_POSE pose2 = { 200, 200, 0, 0, 0, 0 };
	STRUCT_POSE pose3 = { 300, 200, 0, 0, 0, 0 };
	STRUCT_POSE pose4 = { 300, -200, 0, 0, 0, 0 };
	STRUCT_POSE pose5 = { 300, -200, 0, 0, 0, 0 };
	STRUCT_POSE pose6 = { 200, -200, 0, 0, 0, 0 };
	Motion_AddPlaneP2P_Pose(pose1, 0, vel, ovl, ++id, 1);
	for (int i = 0; i < 400; i++){
		Motion_AddPlaneLine_Pose(pose2, 0, vel, ovl, ++id, 1);
		Motion_AddPlaneLine_Pose(pose3, 0, vel, ovl, ++id, 1);
		Motion_AddPlaneLine_Pose(pose4, 0, vel, ovl, ++id, 1);
		Motion_AddPlaneLine_Pose(pose1, 0, vel, ovl, ++id, 1);
		//int ttt = rand() % 4000;
		//printf("t=%d\n", ttt);
		//Sleep(ttt);
		//Motion_CommandPause();
		//Sleep(rand() % 4000);
		//Motion_CommandResume();
		Motion_WaitMoveDone(10, 100000);
	}
	return 0;
}

BOOL CtrlHandler(DWORD fdwCtrlType)
{
	fclose(inter_out);
	if (sclient)closesocket(sclient);
	printf("END-----------\n");
	Sleep(1000);
	return 0;
}

int _tmain(int argc, _TCHAR* argv[])
{
	int ret = 0;
	inter_out = fopen("out.txt", "w+");
	test_fd = fopen("test.txt", "w+");
	SetConsoleCtrlHandler((PHANDLER_ROUTINE)CtrlHandler, TRUE);
	HANDLE hThread1, hThread2;
#ifdef SIMULATE
	InitTcpIp();
#endif
	initial_motion();
	//test_fkine();
	hThread1 = CreateThread(NULL, 0, Serv, &ret, 0, NULL);
	hThread2 = CreateThread(NULL, 0, Pout, &ret, 0, NULL);
	hThread2 = CreateThread(NULL, 0, ToSim, &ret, 0, NULL);
	double posit[MAX_JOINT] = { 0, 0, 0, 0 };
	Motion_SetCurrentPosition(posit);
	Motion_CommandEmergencyRelease();
	Motion_CommandFree();
	//Motion_CommandPlane();
	//Motion_SetCurrentPosition(posit);
	//Motion_CommandPlane();
	test_free();
	//test_plane_line_pose();
	//test_plane_p2p_posit();
	while (1){
		Sleep(1000);
	}
	return 0;
}
